#ifndef INS_Task_H
#define INS_Task_H

#include "struct_typedef.h"
#include "stdint.h"

#define INS_TASK_PERIOD 1

#define INS_YAW_ADDRESS_OFFSET    0
#define INS_PITCH_ADDRESS_OFFSET  1
#define INS_ROLL_ADDRESS_OFFSET   2

#define MPU6500_TEMP_PWM_MAX 			10000

#define TEMPERATURE_PID_KP 1600.0f
#define TEMPERATURE_PID_KI 0.2f
#define TEMPERATURE_PID_KD 0.0f

#define TEMPERATURE_PID_MAX_OUT   9500.0f
#define TEMPERATURE_PID_MAX_IOUT 9400.0f

typedef struct
{
    fp32 MotionAccel_b[3];
    fp32 MotionAccel_n[3];

    fp32 AccelLPF;

    fp32 xn[3];
    fp32 yn[3];
    fp32 zn[3];

    fp32 atanxz;
    fp32 atanyz;
} INS_t;

extern void INS_task(void const *pvParameters);
extern const fp32 *get_INS_quat_point(void);
extern const fp32 *get_INS_angle_point(void);
extern const fp32 *get_gyro_data_point(void);
extern const fp32 *get_accel_data_point(void);

#endif
